#include "pid.h"
#include <string.h>

void PID_Init(pid_t *pid, const pid_config_t *config)
{
    memset(pid, 0, sizeof(pid_t));
    memcpy(&pid->config, config, sizeof(pid_config_t));
}

void PID_Clear(pid_t *pid)
{
    pid->error = 0.0f;
    pid->error_prev = 0.0f;
    pid->error_total = 0.0f;
}

float PID_Update(pid_t *pid)
{
    pid->error_prev = pid->error;

    if (pid->config.Ki != 0.0f)
    {
        if (pid->config.error_total_threshold == 0.0f || pid->error_total < pid->config.error_total_threshold)
            pid->error_total += pid->error;
        else
            pid->error_total = 0.0f;
    }
    else
    {
        pid->error_total = 0.0f;
    }

    float output = pid->config.Kp * pid->error + pid->config.Ki * pid->error_total +
                   pid->config.Kd * (pid->error - pid->error_prev);
    if (output > pid->config.output_max)
        output = pid->config.output_max;
    else if (output < pid->config.output_min)
        output = pid->config.output_min;

    return output;
}